import numpy as np
from riccati import solve_care

M=0.5
m=0.2
l=0.3
g=9.8
a=(M+m)*9.8/M/l
b=1/M/l
c=m*g/M
d=1/M
q1=0
q2=0
q3=1
q4=1
r=1
mA=np.array([[0,1,0,0],[0,0,c,0],[0,0,0,1],[0,0,a,0]])
mB=np.array([[0],[d],[0],[b]])
mQ=np.array([[q1,0,0,0],[0,q2,0,0],[0,0,q3,0],[0,0,0,q4]])
mR=np.array([r])
mP = solve_care(mA, mB, mQ, mR)
K = -1/r * np.dot(mB.T, mP)
K = K[0]
for n in range(K.shape[0]):
    print(K[n], end=', ')
